#ifndef CAN_H_
#define CAN_H_
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>

#include <iostream>
#include <string>
#include <cmath>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include "can_ros1_driver/CarState.h"
// #include "ultralytics_ros/breakState.h"

// #include "mw_lidar/mwlidar.h"
#include "sensor_msgs/Range.h"
#include "sensor_msgs/PointCloud2.h"
#include "radar_perception_msgs/Cluster.h"
#include "radar_perception_msgs/Object.h"

class canControl
{
public:
    // 0 初始化
    canControl();

    // 1 接收CAN消息
    void receiveCanData();

    // 1.1 解析数据
    std::vector<int16_t> hexToShort(const std::vector<uint8_t> &hex_data);

    // 3 接收车子传感器数据，判断传感器是否正常
    // void mwlidarCallback(const mw_lidar::mwlidar::ConstPtr &msg);
    void ultrasonicCallback(const sensor_msgs::Range::ConstPtr &msg);
    void lidarCallback(const sensor_msgs::PointCloud2::ConstPtr &msg);
    // void yoloCallback(const ultralytics_ros::breakState::ConstPtr &msg);

    // 1.2 推算里程计并发布
    void publishOdometry(double linear_velocity, double angular_velocity, double Steer_Angle);

    // 2 析构函数
    ~canControl();

private:
    ros::NodeHandle nh_;
    // 轴距
    double wheel_base;
    // 套接字参数
    struct ifreq ifr;
    struct sockaddr_can can_addr;
    int sockfd;
    int ret;
    // 接收的数据
    struct can_frame frame;

    // can是否使用
    bool can_statu;
    // 毫米波是否使用
    bool mwlidar_statu;
    // 超声波是否使用
    bool ultrasonic_statu;
    // 雷达是否使用
    bool lidar_statu;
    // 相机状态
    bool camera_statu;
    // 用于判断数据是否超时
    double lidar_time, camera_time, mwlidar_time, ultrasonic_time;

    // 当前里程计
    double position_x_;
    double position_y_;
    double yaw;

    radar_perception_msgs::Cluster radar_cluster;
    radar_perception_msgs::Object radar_object;

    // 发布车辆状态和里程计
    ros::Publisher pub_;
    ros::Publisher pub_Odom;
    ros::Publisher pub_radar_cluster;
    ros::Publisher pub_radar_object;
    tf::TransformBroadcaster tf_broadcaster_;
    // 其他传感器订阅
    ros::Subscriber sub_mwlidar, sub_ultrasonic, sub_lidar, sub_yolo;
};

#endif // CAN_H_
